#include "task.h"
#include "sys_log.h"
#include <hal_socket.h>
#include <hal_move.h>
#include "hal_position.h"
#include "hal_eeprom.h"
#include "hal_tlv.h"
#include "gpio.h"

extern struct PacketQueue g_MyQueue;
extern uint8_t g_current_error_flag;
extern MotorMoveSt MotorMove;

static void UartRcvDeal();
static void MotorRunDeal();
static void CurrentErrorDeal();

uint32_t i = 0, j = 0;

void task_motor()
{
    SLOG_I("motor task_motor start \t\r\n");

    // 电衰初始化
    MotorInit();

    while (1)
    {
        if (i++ > MOTOR_APP_LED_FRE)
        {
            HAL_GPIO_TogglePin(APP_LED_GPIOPORT, APP_LED_PIN);
			i = 0;
        }
		if(j++ > MOTOR_APP_LOG_TIMER)
		{
			SLOG_I("motor --------------------task_motor is running-------------------- \t\r\n");
		    j = 0;
		}

        MotorRunDeal();

        UartRcvDeal();

        CurrentErrorDeal();
    }
}

static void UartRcvDeal()
{
    if (!isQueueEmpty(&g_MyQueue))
    {
        struct TLVPacket MyQueue_packet;

        memset(&MyQueue_packet, 0, sizeof(struct TLVPacket));

        // 拷贝cmd length value
        dequeue(&g_MyQueue, &MyQueue_packet);

        ModuleDivide(&MyQueue_packet);
    }
}

static void MotorRunDeal()
{
    uint8_t motor_num       = 0;
    struct TLVPacket packet = {0};

    if (hal_MotorMoveStartFlagGet() == MOVE_STATUES_START)
    {
        SLOG_I("motor MotorRunDeal MOVE_STATUES_START \t\r\n");
        hal_MotorMoveStart();
    }
    else if (hal_MotorMoveStartFlagGet() == MOVE_STATUES_STOP)
    {
        SLOG_I("motor MotorRunDeal MOVE_STATUES_STOP \t\r\n");

        // 开始存储意味着转动逻辑执行完毕
        for (uint8_t i = 1; i <= MOTOR_MAX_ERROR_CHANNEL; i++)
        {
            motor_num = hal_MotorNumRead(i);

            if (motor_num != 0)
            {
                hal_SingleMotorEEPromInformWrite(i, motor_num, EEPSTGTYPE_POSITION);
                hal_SingleMotorEEPromInformWrite(i, motor_num, EEPSTGTYPE_STEPSTATE);
            }
        }

        hal_MotorNumReset();

        hal_MotorMoveStartFlagSet(MOVE_STATUES_NONE);
    }
    else if (hal_MotorMoveStartFlagGet() == MOVE_STATUES_SKIP)
    {
    }
    else if (hal_MotorMoveStartFlagGet() == MOVE_STATUES_NONE)
    {
    }
}

static void CurrentErrorDeal()
{
    if (g_current_error_flag == 1)
    {
        // 在定时器中触发堵转flag会在前台打印
        g_current_error_flag = 0;
        SLOG_I("motor CurrentErrorDeal, number:%d, %d, %d, %d, current: %.4f, %.4f, %.4f, %.4f, over limit: %d \t\r\n", 
        MotorMove.number[0], MotorMove.number[1], MotorMove.number[2], MotorMove.number[3],
        MotorMove.current[0], MotorMove.current[1], MotorMove.current[2], MotorMove.current[3], 
        (uint16_t)MOTOR_CURRENT_LIMIT);
    }
}
